/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include<stdio.h>
#include<stdlib.h>
#include<math.h>
#include "CScanMatchInterface.h"
#include "C3DCorrelationScanMatch.h"
#include "CAHistogramECorrelationScanMatch.h"
#include "ownutils.h"

char in_line[4096];

int readScanLineAvant180(FILE *f,ScanMatch::scan &meas){
	int time_s;
	int time_us;
	int i;
	char c;
	fscanf(f,"%d %d ",&time_s,&time_us);
	//fprintf(stderr,"1 ");
	for(i=0;i<180;i++){
		fscanf(f,"%f",&meas.a[i]);
		fscanf(f,"%c",&c);
   // fprintf(stderr,"%.1f ",meas.r[i]);
		if(c=='\n' && i<180){
			fprintf(stderr,"EOL reached at i=%d\n",i);
			meas.N = i;
			return 0;
		}
	}
	//fprintf(stderr,"2 ");
	for(i=0;i<180;i++){
		fscanf(f,"%f",&meas.r[i]);
		fscanf(f,"%c",&c);
   // fprintf(stderr,"%.1f ",meas.r[i]);
		if(c=='\n' && i<180){
			fprintf(stderr,"EOL reached at i=%d\n",i);
			meas.N = i;
			return 0;
		}
	}
	//fprintf(stderr,"3 \n");
	meas.N = 180;
	return 0;
}

int readScanLineAvant(FILE *f,ScanMatch::scan &meas){
  int index;
  int time_s;
  int time_us;
  int i;
  char c;
  fscanf(f,"%d %d %d ",&index,&time_s,&time_us);
 // fprintf(stderr,"------------------------------------------\n");
  for(i=0;i<361;i++){
    fscanf(f,"%f",&meas.r[i]);
    meas.a[i] = 0.5 *((float) i) * M_PI / 180.0f;
    fscanf(f,"%c",&c);
   // fprintf(stderr,"%.1f ",meas.r[i]);
    if(c=='\n' && i<360){
      fprintf(stderr,"EOL reached at i=%d\n",i);
      meas.N = i;
      return 0;
    }
    
  }
  //fprintf(stderr,"------------------------------------------\n");
  meas.N = 361;
  return 0;
}

int readScanLineJ2B2(FILE *f,ScanMatch::scan &meas,ScanMatch::pose &odo){
    float time;
    int i;
    char c;
    fscanf(f,"%f %f %f %f ",&time,&odo.x,&odo.y,&odo.a);
 // fprintf(stderr,"------------------------------------------\n");
    for(i=0;i<361;i++){
	fscanf(f,"%f",&meas.r[i]);
	meas.a[i] = 0.5 *((float) i) * M_PI / 180.0f;
	fscanf(f,"%c",&c);
   // fprintf(stderr,"%.1f ",meas.r[i]);
	if(c=='\n' && i<360){
	    fprintf(stderr,"EOL reached at i=%d\n",i);
	    meas.N = i;
	    return 0;
	}
    
    }
  //fprintf(stderr,"------------------------------------------\n");
    meas.N = 361;
    return 0;
}

void scan2absoluteXY( ScanMatch::scan &s, float *x, float *y, ScanMatch::pose &pose){
	int i;
	if(x==NULL || y==NULL) return;
	
	for(i=0;i<s.N;i++){
		x[i]=s.r[i]*cos(s.a[i]+pose.a)+pose.x;
		y[i]=s.r[i]*sin(s.a[i]+pose.a)+pose.y;
	}
}

float minMSD(float *x,float *y, float px,float py, int N, int &mini, float &dx2, float &dy2){
	float dist2=100000;
	float d2;
	int minInd = 0;
	float dx,dy;
	for(int i=0;i<N;i++){
		dx = (x[i]-px)*(x[i]-px);
		dy = (y[i]-py)*(y[i]-py);
		d2 = dx + dy;
		if(d2<dist2){ 
			dist2 = d2;
			dx2 = dx;
			dy2 = dy;
			minInd = i;
		}
	}
	mini = minInd;
	return dist2;
}

void computeMSE(ScanMatch::scan &s1,ScanMatch::scan &s2,ScanMatch::pose &p1,ScanMatch::pose &p2){
	float *x1 = (float *) malloc(s1.N*sizeof(float));  
	float *y1 = (float *) malloc(s1.N*sizeof(float));
	float *x2 = (float *) malloc(s2.N*sizeof(float));  
	float *y2 = (float *) malloc(s2.N*sizeof(float));
	
	long int ss1,us1,ss2,us2,sec,us,ms;
	ownTime_GetTimeOfDay(&ss1,&us1);
	
	scan2absoluteXY(s1,x1,y1,p1);
	scan2absoluteXY(s2,x2,y2,p2);
	int mini;
	float dist=0,d;
	float dx2=0,dy2=0,ddx,ddy;
	int num=0;
	for(int i=0;i<s1.N;i++){
		d = minMSD(x2,y2,x1[i],y1[i],s2.N,mini,ddx,ddy);
		if(d<0.5){
		  dist += d; 
			dx2 += ddx;
		  dy2 += ddy;
			num++;
		}
	}	
	
	ownTime_GetTimeOfDay(&ss2,&us2);

	sec = ss2-ss1;
	us = us2-us1;
  ms = sec*1000+us/1000;
	float fms=(float)sec*1000.0+(float)us/1000.0;
	
	fprintf(stderr,"It took %.4f ms to calculate\n",fms);
	
	fprintf(stderr,"MSE= %.2f MSE(X)=%.2f MSE(Y)=%.2f", dist/num,dx2/num,dy2/num);
	fprintf(stderr," ME= %.2f ME(X)=%.2f ME(Y)=%.2f\n", sqrt(dist/num),sqrt(dx2/num),sqrt(dy2/num));
	free(x1);
	free(x2);
	free(y1);
	free(y2);
}


int main(void){
  CScanMatchInterface *matcher;
  ScanMatch::pose pos,odo_cur,odo_ref,est,rpos;   ///< The estimated position
  ScanMatch::scan ref;   ///< The Reference scan 
  ScanMatch::scan meas;  ///< The Reference scan 
  FILE *f,*out;
	float goodnes = 0;
  
  ref.allocate(180);
  meas.allocate(180);
  out = fopen("result.txt","wt");
  //f = fopen("test1.txt","rt");
  //f = fopen("ranging_data.txt","rt");
	//f = fopen("avat180.txt","rt");
	//f = fopen("capture2_hitaastiympariareenaa.txt","rt");
	f = fopen("capture3_jarin_haroily1.txt","rt");
	//f = fopen("capture4_jussin_haroilu1.txt","rt");
	
  if(f==NULL){
    fprintf(stderr,"Error opening data file\n");
    return 1;
  }
  //readScanLineAvant(f,ref);
	readScanLineAvant180(f,ref);
  //readScanLineJ2B2(f,ref,odo_ref);
  pos.set(0,0,0);
  fprintf(stderr,"The reference scan has %d measurements\n",ref.N);
  ///Initializing new matcher
  //matcher = new C3DCorrelationScanMatch();
  /*  
	matcher = new C3DCorrelationScanMatch(4,
                                         4,
                                         6,
                                         0.05,
                                         0.05,
                                         (.5/57.29578),
                                         0.03,
                                         361);*/

  
  matcher = new CAHistogramECorrelationScanMatch(
					 0.5,   // Search area in meters 
					 0.9);     // Search angle in radians
  

  ///< FIXME: tarkistukset interfaceen!! 
  matcher->setReferenceScan(ref, pos); 
 int ind=1;
  while(!feof(f)){
    // Compare two adjacent scans 
    readScanLineAvant180(f,meas);
    //for(int n=0;n<2;n++) 
	//readScanLineJ2B2(f,meas,odo_cur); ///<read measurement with odometry (note odometry is in own coordinate system)
	est = est.calculateDifferentialSensorMovement(odo_ref,odo_cur,M_PI/2.0f); ///calculate differential movement ie initial estimate
	//fprintf(stderr,"odo: (%.2f %.2f %.2f)\n",est.x,est.y,est.a);
	//est.set(0,0,0);
	//matcher->update(meas,est);    ///update
	
	goodnes += matcher->update(meas,pos);    ///update
	matcher->getCurrentPose(pos);
	matcher->setReferenceScan(meas); /// set reference to current position
	fprintf(stderr,"%d: g:%.2f ",ind++,goodnes);
	computeMSE(ref,meas,rpos,pos);
	
	
	for(int i=0;i<meas.N;i++){
		ref.r[i] = meas.r[i];
		ref.a[i] = meas.a[i];
	}
	
	odo_ref.set(odo_cur);///< save the odometry position of the reference
	
	matcher->getCurrentPose(rpos);
	fprintf(out,"%.2f %.2F %.2f ",pos.x,pos.y,pos.a);
	for(int scans=0;scans<meas.N;scans++) fprintf(out,"%.3f ",meas.r[scans]);
	for(int scans=0;scans<meas.N;scans++) fprintf(out,"%.3f ",meas.a[scans]);
	fprintf(out,"\n");
	fflush(out);
    }
  fclose(out);
  fprintf(stderr,"Goodnes: %.2f\n",goodnes);
  /// Deleting
  delete matcher;
  
  return 0;
}
